#include <chrono>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

//期望：
// timer1和timer2各自并行执行，但是各自互斥
//
//
//

class MultiThreadExecutorNode : public rclcpp::Node {
public:
  MultiThreadExecutorNode() : Node("multi_thread_multi_callback_group_node") {
    callback_group1_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);
    // callback_group2_ = this->create_callback_group(
    //     rclcpp::CallbackGroupType::MutuallyExclusive);
    timer1_ = this->create_wall_timer(
        3s,
        [this]() {
          RCLCPP_INFO(this->get_logger(), "Timer 1 is triggered");
          RCLCPP_INFO(this->get_logger(), "Timer 1 is sleeping for 6s..");
          int i = 6;
          while (i--) {
            RCLCPP_INFO(this->get_logger(), "Timer 1 is sleeping for 1s");
            rclcpp::sleep_for(1s);
          }
        },
        callback_group1_);
    // timer2_ = this->create_wall_timer(
    //     1s,
    //     [this]() {
    //       RCLCPP_INFO(this->get_logger(), "Timer 2 is triggered");
    //       RCLCPP_INFO(this->get_logger(), "Timer 2 is sleeping for 2s..");
    //       int i = 2;
    //       while (i--) {
    //         RCLCPP_INFO(this->get_logger(), "Timer 2 is sleeping for 1s");
    //         rclcpp::sleep_for(1s);
    //       }
    //     },
    //     callback_group2_);
  }

private:
  rclcpp::TimerBase::SharedPtr timer1_;
  //   rclcpp::TimerBase::SharedPtr timer2_;
  rclcpp::CallbackGroup::SharedPtr callback_group1_;
  //   rclcpp::CallbackGroup::SharedPtr callback_group2_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MultiThreadExecutorNode>();
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}